#include "control/control_params.h"

#define GET_YAML_FLOAT(KEY,KEY_YAML,DEFAULT_VALUE)      \
    try                                                 \
    {                                                   \
        KEY = yaml_control[KEY_YAML].as<float>(); \
    }                                                   \
    catch (...)                                         \
    {                                                   \
        KEY = DEFAULT_VALUE;                            \
    }

#define GET_YAML_BOOL(KEY,KEY_YAML,DEFAULT_VALUE)      \
    try                                                 \
    {                                                   \
        KEY = yaml_control[KEY_YAML].as<bool>(); \
    }                                                   \
    catch (...)                                         \
    {                                                   \
        KEY = DEFAULT_VALUE;                            \
    }

#define GET_YAML_STRING(KEY,KEY_YAML,DEFAULT_VALUE)      \
    try                                                 \
    {                                                   \
        KEY = yaml_control[KEY_YAML].as<std::string>(); \
    }                                                   \
    catch (...)                                         \
    {                                                   \
        KEY = DEFAULT_VALUE;                            \
    }

#define DEGREETORAD(X) (X)*M_PI/180.0f

std::string g_map_file;//路径地图文件路径
std::string g_vehicle_param_path;//算法参数文件

float g_movefsm_tofinish_dis_precision;
float g_movefsm_tofinish_angle_precision;

// LQR
float g_pose_control_accuracy;// 终点纵向控制精度
float g_lat_error_weight_load;
float g_heading_error_weight_load;
float g_lat_error_rate_weight_load;
float g_heading_error_rate_weight_load; 

float g_lat_error_weight;
float g_heading_error_weight;
float g_lat_error_rate_weight;
float g_heading_error_rate_weight;

float g_lon_preveiwer_dis;
std::string g_speed_plan_type;// 直线使用的加减速模型
float g_slow_down_distance;
float g_slow_down_speed;
float g_position_control_distance;
float g_position_control_kp;

float g_max_acc;//最大加速度
float g_max_dcc;//最大减加速度

float g_headfollowing_min_radius;
float g_turn_min_radius;

float g_curvatrue_control_weight;
float g_max_rotating_angle_speed;
float g_max_rotating_angle_speed_inload;
float g_max_control_angle_speed;
float g_min_rotating_angle_speed;

float g_max_angle_speed_acc;
float g_max_angle_speed_dcc;

float g_f2r_heading_error;
float g_r2f_heading_error;

float g_zero_speed;//零速阈值
float g_min_move_speed;
float g_dec_vel_error_k;
float g_dec_response_delay;
float g_f2r_pose_thea_path_angle_diff_thresh;
float g_search_path_id_dis_thresh;// 搜索path_id时的距离阈值
std::vector<std::pair<float, float>> g_v_search_path_id_angle_thresh;
float g_search_path_id_angle_thresh;
float g_pick_up_length;
float g_pathid_valid_dis;

float g_online_angle;
float g_cfc_rotating_kp;

// cfc f2r s2f f2s judge threshold

// threshold of next line angle with steer angle in global coord.
float g_f2s_line_theta_steer_angle_thresh;
// threshold of steer angle diff with headfollowing speed toward
float g_f2s_actuator_steer_speed_toward_threshold;
// threshold of cur angle diff with point targe angle
float g_f2s_cur_angle_target_angle_threshold;
// steer to follow
float g_s2f_steer_angle_speed_toward_diff_threshold;
float g_s2f_global_steer_angle_diff_threshold;
float g_s2f_speed_toward_line_angle_diff_threshold;

float g_cfc_max_adjust_speed_toward;
float g_cfc_max_speed_toward;

float g_creep_distance;

std::map<float, float> g_cfc_heading_error_weight_map_;

void LoadGlobalParams(const std::string &path, const std::string &yaml_file)
{
    YAML::Node vehicle_param_yaml = YAML::LoadFile(yaml_file);
    std::cout << "yaml_file: " << yaml_file.c_str() << std::endl;
    YAML::Node yaml_control = vehicle_param_yaml["amr_control"];

    GET_YAML_FLOAT(g_movefsm_tofinish_dis_precision, "movefsm_tofinish_dis_precision", 0.005f);
    GET_YAML_FLOAT(g_movefsm_tofinish_angle_precision, "movefsm_tofinish_angle_precision_degree", 1.0f);
    GET_YAML_FLOAT(g_pose_control_accuracy, "pose_control_accuracy", 0.01f);

    GET_YAML_FLOAT(g_lat_error_weight, "lat_error_weight", 1.0f);
    GET_YAML_FLOAT(g_heading_error_weight, "heading_error_weight", 1.0f);
    GET_YAML_FLOAT(g_lat_error_rate_weight, "lat_error_rate_weight", 0.01f);
    GET_YAML_FLOAT(g_heading_error_rate_weight, "heading_error_rate_weight", 0.01f);

    GET_YAML_FLOAT(g_lat_error_weight_load, "lat_error_weight_load", g_lat_error_weight);
    GET_YAML_FLOAT(g_heading_error_weight_load, "heading_error_weight_load", g_heading_error_weight);
    GET_YAML_FLOAT(g_lat_error_rate_weight_load, "lat_error_rate_weight_load", g_lat_error_rate_weight);
    GET_YAML_FLOAT(g_heading_error_rate_weight_load, "heading_error_rate_weight_load", g_heading_error_rate_weight);

    GET_YAML_FLOAT(g_lon_preveiwer_dis, "lon_preveiwer_dis", 0.0f);
    GET_YAML_FLOAT(g_slow_down_distance, "slow_down_distance", 0.0f);
    GET_YAML_FLOAT(g_slow_down_speed, "slow_down_speed", 0.005f);
    GET_YAML_FLOAT(g_position_control_distance, "position_control_distance", 2.0f);
    GET_YAML_FLOAT(g_position_control_kp, "position_control_kp", 0.5f);

    GET_YAML_FLOAT(g_max_acc, "max_acc", 0.5f);
    GET_YAML_FLOAT(g_max_dcc, "max_dcc", 0.5f);

    GET_YAML_FLOAT(g_headfollowing_min_radius, "headfollowing_min_radius", 0.1f);
    GET_YAML_FLOAT(g_turn_min_radius, "turn_min_radius", 0.0f);

    GET_YAML_FLOAT(g_curvatrue_control_weight, "curvatrue_control_weight", 1.0f);
    GET_YAML_FLOAT(g_max_rotating_angle_speed, "max_rotating_angle_speed_degree", 90.0f);
    GET_YAML_FLOAT(g_max_rotating_angle_speed_inload, "max_rotating_angle_speed_degree_inload", 90.0f);
    GET_YAML_FLOAT(g_max_control_angle_speed, "max_control_angle_speed_degree", 45.0f);
    GET_YAML_FLOAT(g_min_rotating_angle_speed, "min_rotating_angle_speed_degree", 1.0f);

    GET_YAML_FLOAT(g_max_angle_speed_acc, "max_angle_speed_acc_degree", 0.5f);
    GET_YAML_FLOAT(g_max_angle_speed_dcc, "max_angle_speed_dcc_degree", 0.5f);

    GET_YAML_FLOAT(g_f2r_heading_error, "f2r_heading_error_degree", 10.0f);
    GET_YAML_FLOAT(g_r2f_heading_error, "r2f_heading_error_degree", 1.0f);

    GET_YAML_FLOAT(g_zero_speed, "zero_speed", 0.001f);
    GET_YAML_FLOAT(g_min_move_speed, "min_move_speed", 0.005f);
    GET_YAML_FLOAT(g_dec_vel_error_k, "dec_vel_error_k", 1.0f);
    GET_YAML_FLOAT(g_dec_response_delay, "dec_response_delay", 0.005f);
    GET_YAML_FLOAT(g_f2r_pose_thea_path_angle_diff_thresh, "f2r_pose_thea_path_angle_diff_thresh", 10.0f);

    GET_YAML_FLOAT(g_search_path_id_dis_thresh, "search_path_id_dis_thresh", 0.5f);
    GET_YAML_FLOAT(g_pick_up_length, "pick_up_length", 2.0f);
    GET_YAML_FLOAT(g_pathid_valid_dis, "pathid_valid_dis", 0.1f);

    GET_YAML_FLOAT(g_search_path_id_angle_thresh, "search_path_id_angle_thresh_degree", 30.0f);
    GET_YAML_FLOAT(g_online_angle, "online_angle_degree", 30.0f);
    GET_YAML_FLOAT(g_cfc_rotating_kp, "cfc_rotating_kp", 1.0f);

    GET_YAML_FLOAT(g_cfc_max_adjust_speed_toward, "cfc_max_adjust_speed_toward_degree", 5.0f);
    GET_YAML_FLOAT(g_cfc_max_speed_toward, "cfc_max_speed_toward_degree", 95.0f);

    GET_YAML_FLOAT(g_creep_distance, "creep_distance", 0.0f);
    
    if (yaml_control["search_path_id_angle_thresh"].IsDefined())
    {
        YAML::Node threshes = yaml_control["v_search_path_id_angle_thresh"];
        auto it = threshes.begin();
        for(; it != threshes.end(); it++)
        {
            g_v_search_path_id_angle_thresh.push_back(std::make_pair<float, float>((*it)["begin"].as<float>(), (*it)["end"].as<float>()));
        }
    }
    else
    {
        g_v_search_path_id_angle_thresh.push_back(std::make_pair<float, float>(0/ 57.3, 30 / 57.3));
    }

    if (yaml_control["cfc_heading_error_weight_map"].IsDefined())
    {
        for (int i=0;i<yaml_control["cfc_heading_error_weight_map"].size();i++)
        {
            YAML::Node pair = yaml_control["cfc_heading_error_weight_map"][i];
            g_cfc_heading_error_weight_map_.insert(std::pair<float, float>(pair["velocity"].as<float>(), pair["weight_param"].as<float>()));
        }
    }
    else
    {
        g_cfc_heading_error_weight_map_[0.1f] = 0.20f;
        g_cfc_heading_error_weight_map_[0.2f] = 0.25f;
        g_cfc_heading_error_weight_map_[0.5f] = 1.00f;
    }

    g_f2r_heading_error = DEGREETORAD(g_f2r_heading_error);
    g_r2f_heading_error = DEGREETORAD(g_r2f_heading_error);

    g_movefsm_tofinish_angle_precision = DEGREETORAD(g_movefsm_tofinish_angle_precision);
    g_search_path_id_angle_thresh = DEGREETORAD(g_search_path_id_angle_thresh);
    g_online_angle = DEGREETORAD(g_online_angle);

    g_max_rotating_angle_speed = DEGREETORAD(g_max_rotating_angle_speed);
    g_max_rotating_angle_speed_inload = DEGREETORAD(g_max_rotating_angle_speed_inload);
    g_max_control_angle_speed = DEGREETORAD(g_max_control_angle_speed);
    g_min_rotating_angle_speed = DEGREETORAD(g_min_rotating_angle_speed);

    g_cfc_max_adjust_speed_toward = DEGREETORAD(g_cfc_max_adjust_speed_toward);
    g_cfc_max_speed_toward = DEGREETORAD(g_cfc_max_speed_toward);

}